package endcourseproject;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class EyeSensor extends Thread{

	
	public static void main(String[] args) 
	{
		EyeSensor es = new EyeSensor(Motor.A, new UltrasonicSensor(SensorPort.S3));
		
		
		while(!Button.ESCAPE.isPressed())
		{
			LCD.drawString("Left: " + es.getDistanceLeft() + "    ", 0, 0 );
			LCD.drawString("Right: " + es.getDistanceRight() + "    ", 0, 1 );
			
			Thread.yield();
		}
	}

	
	private Motor _motor = null;
	private UltrasonicSensor _sensor = null;
	
	private static final int DEGRESS_TO_TURN = 90;
	
	
	public EyeSensor(Motor motor, UltrasonicSensor sensor)
	{
		_motor = motor;
		_sensor = sensor;
		setDaemon(true);
		start();
	}
	
	
	private void lookLeft()
	{
		_motor.rotate(DEGRESS_TO_TURN*2, false);
	}
	
	private void lookRight()
	{
		_motor.rotate(-(DEGRESS_TO_TURN*2), false);
	}
	
	private int look()
	{
		_sensor.ping();
		Common.sleep(100);
		return _sensor.getDistance();
	}
	
	private int _distanceLeft = -1;
	private int _distanceRight = -1;
	
	public int getDistanceLeft()
	{
		return _distanceLeft;
	}
	public int getDistanceRight()
	{
		return _distanceRight;
	}
	
	public void run()
	{
		_motor.rotate(-DEGRESS_TO_TURN, false);
		while(true)
		{
			lookLeft();
			_distanceLeft = look();
			//Common.sleep(10);
			lookRight();
			_distanceRight = look();
			//Common.sleep(10);
		}
	}
	
}
